#include "Code7_MOTION_FUNCTION.h"
#include "math.h" 
																				
unsigned char WaistAngle_Low = 0;
unsigned char WaistAngle_High = 0;
int WaistAngle = 2048; // the current position of waist 

/*******************************************************************************
* Function Name  : Mtn_RotateWaist
* Description    : This Motion is used in the "Basketball" event.
* Input          : Turning angle received from COMP.
* Output         : None
* Return         : None
* Attention		 	 : The unit of turning angle is degree. If turning angle is larger
									 than 30 or less than -30, then this command will be ignored.
									 Besides, the angle of waist is bounded by 1190 to 2906.
*******************************************************************************/	
void Mtn_RotateWaist(float angle)
{
	unsigned char angle_low,angle_high = 0;									
	
//	if((angle <= 30) && (angle >= -30))
//	{ // limit the angle of waist
//	 WaistAngle = WaistAngle - (int)(angle * 14.3);
//
//	 if(WaistAngle < 1190){WaistAngle = 1190;}
//	 if(WaistAngle > 2906){WaistAngle = 2906;}
//	 angle_low  = WaistAngle & 0xff; // (1024 / 300 = 3.413)
//	 angle_high = WaistAngle >> 8;
//	 
//	 Add_motor_job(  253,  10,   0,   0,   0 );
//	 Add_motor_job(    3,  angle_low,  angle_high, 150, 0 );
//	 Add_motor_job(  255,   5,   0,   0,   0 );
//	}
	//printf("Angle: %d \r\n", (int)(angle*100));
	if((angle <= 40) && (angle >= -40))
	{
		WaistAngle = 2048 - (int)(3 * angle / MX106_Resolution);
		angle_low  = WaistAngle & 0x00ff;
		angle_high = (WaistAngle & 0xff00) >> 8;

		Add_motor_job(  253,  10,   0,   0,   0 );
		Add_motor_job(    4,  angle_low,  angle_high, 150, 0 );
		Add_motor_job(  255,   5,   0,   0,   0 );
		//printf("Waist Angle: %d, Low: %d, High: %d\r\n", WaistAngle, (int)angle_low, (int)angle_high);
	}
}


/*******************************************************************************
* Function Name  : Mtn_Throw
* Description    : This Motion is used in the "Basketball" event.
* Input          : Throwing distance received from COMP.
* Output         : None
* Return         : None
* Attention		 	 : The unit of throwing distance is cm. The speed and angle of 
									 some motors are calculated by pre-fitting curve. 
									 Besides, the throwing distance is bounded by 105 to 200.
*******************************************************************************/	
void Mtn_Throw(float throw_distance)
{
  double ID12_angle = 0;// ID 12 motor angle
  double ID13_angle  = 0;//ID 13 motor angle
  double ID14_speed = 0;// ID 14 motor speed			
  unsigned char ID12_angle_L, ID12_angle_H = 0;
  unsigned char ID13_angle_L, ID13_angle_H = 0;				 
  unsigned char ID14_speed_L, ID14_speed_H = 0;			

  throw_distance = (double)(throw_distance);

  if(throw_distance > 170) {throw_distance = 170;}
  else if(throw_distance < 110) {throw_distance = 110;}

  if(throw_distance > 130)
  {
   ID12_angle = 20*(throw_distance - 130)/40 + 792;// linear motor angle with distance 170:130 , angle 812:792
   
   ID14_speed = -0.000166666666667 * pow(throw_distance,4) 
   				+ 0.101666666666628 * pow(throw_distance,3) 
				- 23.233333333324000 * pow(throw_distance,2) 
				+ 2361.333333332300000 * throw_distance 
				- 90059.999999955700000;
 
    // Seperate to low / high byte
  			ID12_angle_L = (int)(ID12_angle) & 0xff;
  			ID12_angle_H = (int)(ID12_angle) >> 8;

  			ID14_speed_L = (int)(ID14_speed) & 0xff;
  			ID14_speed_H = (int)(ID14_speed) >> 8;

			Add_motor_job(  253,  10,   0,   0,   0 );
		   	Add_motor_job(   11,  30,   8,  28,   0 );
		   	Add_motor_job(   10,  56,   7, 188,   0 );
		   	Add_motor_job(   12,  49,   3, 173,   1 );
		   	Add_motor_job(   14,  74,   3,  55,   1 );
		   	Add_motor_job(  255,   2,   0,   0,   0 );
		   	Add_motor_job(    3,  56,   7,  20,   0 );
		   	Add_motor_job(   11, 120,   8,   9,   0 );
		   	Add_motor_job(   10, 226,  12, 152,   0 );
		   	Add_motor_job(   13,  62,   6,  47,   0 );
		   	Add_motor_job(   14, 204,   3,  50,   0 );
		   	Add_motor_job(  255,  36,   0,   0,   0 );
		   	Add_motor_job(   14, 210,   2,  ID14_speed_L,   ID14_speed_H);
		   	Add_motor_job(  255,   2,   0,   0,   0 );
		   	Add_motor_job(   10, 130,   3, 132,   3 );
		   	Add_motor_job(   12,  ID12_angle_L,   ID12_angle_H,  30,   0 );
		   	Add_motor_job(   13, 147,  11,  88,   2 );
		   	Add_motor_job(   14,  76,   1, 194,   1 );
		   	Add_motor_job(  255,   6,   0,   0,   0 );
			Add_motor_job(   11,   0,   8,  17,   0 );
		    Add_motor_job(   12,   0,   2, 100,   0 );
		    Add_motor_job(   13,   0,   8, 100,   0 );
		    Add_motor_job(   14,   0,   2,  70,   0 );
		    Add_motor_job(  255,  20,   0,   0,   0 );
		    Add_motor_job(    3,   0,   8,  37,   0 );
		    Add_motor_job(   10,   0,   8, 150,   0 );
			Add_motor_job(  255,  10,   0,   0,   0 );
	}
/*						 															
  double ID12_angle = 0;// ID 12 motor speed
  double ID13_angle  = 0;//ID 13 motor angle			
  unsigned char ID12_angle_L, ID12_angle_H = 0;
  unsigned char ID13_angle_L, ID13_angle_H = 0;				 
			

  throw_distance = (double)(throw_distance);

  if(throw_distance > 170) {throw_distance = 170;}
  else if(throw_distance > 150)
  {
  throw_distance = throw_distance + 5;
  }
  else if(throw_distance < 140 && throw_distance > 110)
  {
  	throw_distance = throw_distance + 10;
  }
  else if (throw_distance < 110) {throw_distance = 110;}
  
//	if(throw_distance > 132)
//	{
//  	throw_distance = throw_distance - 12; // Correction: ori 0
//	}
//	else if(throw_distance > 128)
//	{										
//  	throw_distance = throw_distance - 8; 
//	}
//	else
//	{
//		throw_distance = throw_distance - 6;
//	}
	if(throw_distance > 125)
	{						 
		  ID12_angle =  0.0000312500 * pow(throw_distance,4) 
		  				- 0.0182638889 * pow(throw_distance,3) 
						+ 3.9822916667 * pow(throw_distance,2) 
						- 383.0634920641 * throw_distance 
						+ 14474.6785714427; 

						 
		  ID13_angle = 	0.00018750000000219100 * pow(throw_distance,4) 
		  				- 0.11458333333465400000 * pow(throw_distance,3) 
						+ 26.20625000030450000000 * pow(throw_distance,2) 
						- 2659.79166669853000000000 * throw_distance 
						+ 104113.00000127300000000000; 
 

		  //Bound
  
		  if(ID12_angle > 817){ID12_angle = 817;}

		  // Seperate to low / high byte
  			ID12_angle_L = (int)(ID12_angle) & 0xff;
  			ID12_angle_H = (int)(ID12_angle) >> 8;

  			ID13_angle_L = (int)(ID13_angle) & 0xff;
  			ID13_angle_H = (int)(ID13_angle) >> 8;



		  //Motion of Basketball_far
		   Add_motor_job(  253,  10,   0,   0,   0 );
		   Add_motor_job(   11,  30,   8,  28,   0 );
		   Add_motor_job(   10,  56,   7, 188,   0 );
		   Add_motor_job(   12,  49,   3, 173,   1 );
		   Add_motor_job(   14,  74,   3,  55,   1 );
		   Add_motor_job(  255,   2,   0,   0,   0 );
		   Add_motor_job(    3,  56,   7,  20,   0 );
		   Add_motor_job(   11, 120,   8,   9,   0 );
		   Add_motor_job(   10, 226,  12, 152,   0 );
		   Add_motor_job(   13,  62,   6,  47,   0 );
		   Add_motor_job(   14, 204,   3,  50,   0 );
		    if(throw_distance < 167)
		    {
		   		Add_motor_job(   20, 232,  11, 104,   0 );
			}
		   Add_motor_job(  255,  40,   0,   0,   0 );
		   Add_motor_job(   14, 240,   2, 230,   0 );
		   Add_motor_job(  255,   2,   0,   0,   0 );
		   Add_motor_job(   10, 130,   3, 132,   3 );
		   Add_motor_job(   12,   ID12_angle_L,   ID12_angle_H,  30,   0 );
		   Add_motor_job(   13,   ID13_angle_L,   ID13_angle_H,  88,   2 );
		   Add_motor_job(   14,  76,   1, 194,   1 );
		    if(throw_distance < 167)
		    {
		   		Add_motor_job(   20,  12,   6,   88,  2);
			}
		   Add_motor_job(  255,  10,   0,   0,   0 );
		   Add_motor_job(   11,   0,   8,  17,   0 );
		   Add_motor_job(   12,   0,   2, 100,   0 );
		   Add_motor_job(   13,   0,   8, 100,   0 );
		   Add_motor_job(   14,   0,   2,  70,   0 );
		   Add_motor_job(  255,  20,   0,   0,   0 );
		   Add_motor_job(    3,   0,   8,  37,   0 );
		   Add_motor_job(   10,   0,   8, 150,   0 );
		    if(throw_distance < 167)
		    {
				Add_motor_job(   20,   0,   8,  94,   0 );
			}
		   Add_motor_job(  255,  10,   0,   0,   0 );
	}
	*/
	else
	{
	    ID12_angle = 812;
		
		ID13_angle = -0.050000000000013 * pow(throw_distance,2)
					 + 8.500000000003310 * throw_distance 
					 + 2807.999999999800000; 

		 
	   // Seperate to low / high byte
  			
  			ID12_angle_L = (int)(ID12_angle) & 0xff;
  			ID12_angle_H = (int)(ID12_angle) >> 8;

  			ID13_angle_L = (int)(ID13_angle) & 0xff;
  			ID13_angle_H = (int)(ID13_angle) >> 8;

		//Motion of Basketball_near
		   Add_motor_job(  253,  10,   0,   0,   0 );
		   Add_motor_job(   11,  30,   8,  28,   0 );
		   Add_motor_job(   10,  56,   7, 188,   0 );
		   Add_motor_job(   12,  49,   3, 173,   1 );
		   Add_motor_job(   14,  74,   3,  55,   1 );
		   Add_motor_job(  255,   2,   0,   0,   0 );
		   Add_motor_job(    3,  56,   7,  20,   0 );
		   Add_motor_job(   11, 120,   8,   9,   0 );
		   Add_motor_job(   10, 226,  12, 152,   0 );
		   Add_motor_job(   13,   6,   7,  26,   0 );
		   Add_motor_job(   14, 204,   3,  50,   0 );
		   Add_motor_job(   20, 232,  11, 104,   0 );
		   Add_motor_job(  255,  41,   0,   0,   0 );
		   Add_motor_job(   14, 240,   2, 230,   0 );
		   Add_motor_job(  255,   2,   0,   0,   0 );
		   Add_motor_job(   10,  74,   4,  88,   2 );
		   Add_motor_job(   12,  ID12_angle_L,   ID12_angle_H,  30,   0 );
		   Add_motor_job(   13,  ID13_angle_L,   ID13_angle_H, 144,   1 );
		   Add_motor_job(   14, 236,   1,  94,   1 );
		   Add_motor_job(   20,  12,   6, 194,   1 );
		   Add_motor_job(  255,  10,   0,   0,   0 );
		   Add_motor_job(   11,   0,   8,  17,   0 );
		   Add_motor_job(   12,   0,   2, 100,   0 );
		   Add_motor_job(   13,   0,   8, 100,   0 );
		   Add_motor_job(   14,   0,   2,  70,   0 );
		   Add_motor_job(  255,  20,   0,   0,   0 );
		   Add_motor_job(    3,   0,   8,  37,   0 );
		   Add_motor_job(   10,   0,   8, 150,   0 );
		   Add_motor_job(   20,   0,   8,  94,   0 );
		   Add_motor_job(  255,  10,   0,   0,   0 );
	}						
	
							 
	
//	USART_SendData(DEBUG_PORT, Master_Receive[MASTER_R_CONT_ANGLE_L]);    
//	while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TXE) == RESET)
//	{
//	}					
//	USART_SendData(DEBUG_PORT, Master_Receive[MASTER_R_CONT_ANGLE_H]);    
//	while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TXE) == RESET)
//	{
//	}	
//	if(Motion.ContinueAngle_RX > 0)
//	{					
//		USART_SendData(DEBUG_PORT, 0);    
//		while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TXE) == RESET)
//		{
//		}		
//	}
//	else
//	{			
//		USART_SendData(DEBUG_PORT, 1);    
//		while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TXE) == RESET)
//		{
//	}		
//	}				
//	USART_SendData(DEBUG_PORT, abs((int)Motion.ContinueAngle_RX));    
//	while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TXE) == RESET)
//	{
//	}																	 
	USART_SendData(DEBUG_PORT, (unsigned char)throw_distance);    
	while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TXE) == RESET)
	{
	}												
//
//	USART_SendData(DEBUG_PORT, ID6_angle_L);    
//	while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TXE) == RESET)
//	{
//	}																					
//	USART_SendData(DEBUG_PORT, ID6_angle_H);    
//	while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TXE) == RESET)
//	{
//	}											
//			
//	USART_SendData(DEBUG_PORT, ID6_speed_L);    
//	while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TXE) == RESET)
//	{
//	}
//	USART_SendData(DEBUG_PORT, ID6_speed_H);    
//	while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TXE) == RESET)
//	{
//	}
//
//	USART_SendData(DEBUG_PORT, ID8_speed_L);    
//	while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TXE) == RESET)
//	{
//	}
//	USART_SendData(DEBUG_PORT, ID8_speed_H);    
//	while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TXE) == RESET)
//	{
//	}
//
//	USART_SendData(DEBUG_PORT, ID9_speed_L);    
//	while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TXE) == RESET)
//	{
//	}
//	USART_SendData(DEBUG_PORT, ID9_speed_H);    
//	while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TXE) == RESET)
//	{
//	}
}


/*******************************************************************************
* Function Name  : MtnC_Waist_F
* Description    : This Motion is used in the continuous motion - "MtnC_Walk_F".
* Input          : None
* Output         : None
* Return         : None
* Attention		 	 : None
*******************************************************************************/	
void MtnC_Waist_F(void)
{						 
	Add_motor_job(	255,   5,		0,	 0,		0 );
	Add_motor_job(    2,  56,   7,  88,   0 );			 
	Add_motor_job(	255,  15,		0,	 0,		0 );
}
																						
/*******************************************************************************
* Function Name  : MtnC_Waist_B
* Description    : This Motion is used in the continuous motion - "MtnC_Walk_B".
* Input          : None
* Output         : None
* Return         : None
* Attention		 	 : None
*******************************************************************************/	
void MtnC_Waist_B(void)
{										 
	Add_motor_job(	255,   5,		0,	 0,		0 );
	Add_motor_job(    2,  46,   7,  88,   0 );				
	Add_motor_job(	255,  15,		0,	 0,		0 ); 
}
